#!/usr/bin/env python
# -*- coding:utf-8 -*-

import binascii
import hashlib
import os
import math
import struct
import thread
import threading
import time
import ctypes as ct
import rospy
import serial
from std_msgs.msg import Float32, Float32MultiArray, String
import handsfree_arm_msgs.msg as handsfree_arm_msg
import handsfree_arm_msgs.srv as handsfree_arm_srv


def int2hexStr(int_data):
    return hex(int_data | 0xff00)[4:6]


# 接受数据的拆分
def acceptByte(accept_data):
    accept_byte = []
    accept_len = len(accept_data)
    for i in range(0, accept_len, 2):
        accept_byte.append(accept_data[i:i + 2])
    return accept_byte


# 16 转 10进制
def hex2dec(hex):
    return int(hex, 16)


# 调用 C++ 中的方法
def shift_left(high, low):
    return ct.c_int16(high << 8 | low).value


# 机械臂反馈的 "关节位置" 给上位机,零位校准处理,角度制
def hwArmAngles2RosAngles(angles_list_):
    ros_angles_lists_ = [0, 0, 0, 0, 0, 0]

    ros_angles_lists_[0] = angles_list_[0]
    ros_angles_lists_[1] = angles_list_[1]
    ros_angles_lists_[2] = (108.71 - angles_list_[2])
    ros_angles_lists_[3] = (-angles_list_[3])
    ros_angles_lists_[4] = (-angles_list_[4])
    ros_angles_lists_[5] = angles_list_[5]
    return ros_angles_lists_


# 上位机发送的 "关节位置" 给机械臂,零位校准处理，角度制
def hwRosAngles2ArmAngles(angles_list_):
    hw_angles_list_ = [0, 0, 0, 0, 0, 0]

    hw_angles_list_[0] = angles_list_[0]
    hw_angles_list_[1] = angles_list_[1]
    hw_angles_list_[2] = (108.71 - angles_list_[2])
    hw_angles_list_[3] = (-angles_list_[3])
    hw_angles_list_[4] = (-angles_list_[4])
    hw_angles_list_[5] = angles_list_[5]
    return hw_angles_list_


# 机械臂反馈的 "关节位置" 给上位机,零位校准处理,角度制
def hwArmGripper2RosGripper(data_):
    return (data_ - 500) / 2000


# 上位机发送的 "夹爪位置" 给机械臂 , 0~1
def hwRosGripper2ArmGripper(data_):
    return 500 + data_ * 2000


# 关节限幅，角度制
def hwArmAnglesLimt(angles_):
    angles_limt_ = angles_

    # 零位 0.0 ,角度范围[-141, 179.5]
    if angles_[0] < -141:
        angles_limt_[0] = -140.5
        print ("关节0控制数据超出下限 angles_limt[0] = %f" % angles_limt_[0])
    elif angles_[0] > 179.5:
        angles_limt_[0] = 179
        print ("关节0控制数据超出上限 angles_limt[0] = %f" % angles_limt_[0])

    # 零位 0.0 ,角度范围[-65.5, 102]
    if angles_[1] < -65.5:
        angles_limt_[1] = -65
        print ("关节1控制数据超出下限 angles_limt[1] = %f" % angles_limt_[1])
    elif angles_[1] > 102:
        angles_limt_[1] = 101.5
        print ("关节1控制数据超出上限 angles_limt[1] = %f" % angles_limt_[1])

    # 零位 0.0 + 108.71 ,角度范围 [-82.7, 79.3], 添加负号方向相反,则角度范围变为[-79.3,82.7]
    if angles_[2] < -79.3:
        angles_limt_[2] = 78.8
        print ("关节2控制数据超出下限 angles_limt[2] = %f" % angles_limt_[2])
    elif angles_[2] > 82.7:
        angles_limt_[2] = 82.5
        print ("关节2控制数据超出上限 angles_limt[2] = %f" % angles_limt_[2])

    # 零位 0.0, 角度范围[-123 ,168.8], 添加负号方向相反, 则角度范围变为[-168.8,123]
    if angles_[3] < -168.8:
        angles_limt_[3] = -168.5
        print ("关节3控制数据超出下限 angles_limt[3] = %f" % angles_limt_[3])
    elif angles_[3] > 123:
        angles_limt_[3] = 122.5
        print ("关节3控制数据超出上限 angles_limt[3] = %f" % angles_limt_[3])

    # 零位 0.0 角度范围[-34,209], 添加负号方向相反, 则角度范围变为[-209,34]
    if angles_[4] < -209:
        angles_limt_[4] = -208.5
        print ("关节4控制数据超出下限 angles_limt[4] = %f" % angles_limt_[4])
    elif angles_[4] > 34:
        angles_limt_[4] = 33.5
        print ("关节4控制数据超出上限 angles_limt[4] = %f" % angles_limt_[4])

    # 零位 0.0 角度范围[-270,270]
    if angles_[5] < -270:
        angles_limt_[5] = -269.5
        print ("关节5控制数据超出下限 angles_limt[5] = %f" % angles_limt_[5])
    elif angles_[5] > 270:
        angles_limt_[5] = 269.5
        print ("关节5控制数据超出上限 angles_limt[5] = %f" % angles_limt_[5])

    return angles_limt_


# 设置机械臂关节角度, 弧度制
def hwSetJointsPosition(joint_pose_list_):
    # 存储接受到的角度
    angle = [0, 0, 0, 0, 0, 0]

    gripper_ = 0
    if len(joint_pose_list_) == 6:
        for i in range(6):
            angle[i] = (joint_pose_list_[i] * 180 / math.pi)  # 弧度转角度

        gripper_ = port_read_joints[6]  # gripper_ 保持当前状态值不变
    elif len(joint_pose_list_) == 7:
        for i in range(6):
            angle[i] = (joint_pose_list_[i] * 180 / math.pi)  # 弧度转角度

        gripper_ = hwRosGripper2ArmGripper(joint_pose_list_[6])  # 转换成机械臂的夹爪数据
    else:
        rospy.loginfo('数组长度错误，请输入 6 个或者７个有效32位浮点数')
        return

    print (angle)

    # 限幅处理
    angle = hwArmAnglesLimt(angle)

    # 转换成机械臂的关节数据
    angle = hwRosAngles2ArmAngles(angle)

    # 使用的是串行指令轴角度补插, S6H4D_串行指令详解_2020c.pdf 文档 3、轴角度插补
    data = 'ee33' + arm_buffer + binascii.b2a_hex(struct.pack('<f', angle[0])) + binascii.b2a_hex(
        struct.pack('<f', angle[1])) + binascii.b2a_hex(struct.pack('<f', angle[2])) + binascii.b2a_hex(
        struct.pack('<f', angle[3])) + binascii.b2a_hex(struct.pack('<f', angle[4])) + binascii.b2a_hex(
        struct.pack('<f', angle[5])) + binascii.b2a_hex(
        struct.pack('<f', gripper_)) + '000000000000000000000000' + binascii.b2a_hex(
        struct.pack('<f', float(arm_speed))) + 'ef'

    time.sleep(0.001)
    hf_arm.write(binascii.a2b_hex(data))
    hf_arm.flush()


# (夹抓 0~1, 1:开到最大, 0:合到最小)
def hwSetJointGripper(data_):
    if data_ < 0:
        data_ = 0
    elif data_ > 1:
        data_ = 1

    joint_pose_list_ = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

    for i in range(6):
        joint_pose_list_[i] = joints_position[i]
    joint_pose_list_[6] = data_

    print ("set joint gripper to %f" % data_)
    hwSetJointsPosition(joint_pose_list_)


# (吸盘 0~1, 0:关，1:开)
def hwSetJointSucker(data_):
    if data_ < 0.5:
        data_ = 0
    else:
        data_ = 1

    sucker_pos = '00'
    if data_ == 1:
        sucker_pos = '01'
        print("sucker open")
    else:
        print("sucker release")

    # 使用的是 M 代码指令, S6H4D_串行指令详解_2020c.pdf 文档      11、轴角度插补, 00 和 01 ，关和开
    data = 'fc1600' + sucker_pos + '00000000000000000000000000000000000000000000000000000000000000000000000000000000000000fd'
    hf_arm.write(binascii.a2b_hex(data))


# 解析反馈的数据包
def hwSerialDataLoopAnalysis(port):
    count = port.inWaiting()
    if count > 10:
        accept_data = acceptByte(binascii.b2a_hex(port.read(count)))

        if accept_data[0] == 'ce' and accept_data[10] == 'cf':
            if accept_data[9] == '00':
                port_read_state[0] = True
                port_read_x = (float(shift_left(hex2dec(accept_data[1]), hex2dec(accept_data[2]))) / 10)
                port_read_y = (float(shift_left(hex2dec(accept_data[3]), hex2dec(accept_data[4]))) / 10)
                port_read_z = (float(shift_left(hex2dec(accept_data[5]), hex2dec(accept_data[6]))) / 10)
                #print port_read_x,port_read_y,port_read_z
                port_read_joints[0] = (float(shift_left(hex2dec(accept_data[7]), hex2dec(accept_data[8]))) / 100)
            if accept_data[9] == '01':
                port_read_state[1] = True
                port_read_joints[1] = (float(shift_left(hex2dec(accept_data[1]), hex2dec(accept_data[2]))) / 100)
                port_read_joints[2] = (float(shift_left(hex2dec(accept_data[3]), hex2dec(accept_data[4]))) / 100)
                port_read_b0 = (float(shift_left(hex2dec(accept_data[5]), hex2dec(accept_data[6]))) / 100)
                port_read_b1 = (float(shift_left(hex2dec(accept_data[7]), hex2dec(accept_data[8]))) / 100)
                #print 'b0,b1',port_read_b0,port_read_b1
            if accept_data[9] == '02':
                port_read_state[2] = True
                port_read_joints[5] = (float(shift_left(hex2dec(accept_data[1]), hex2dec(accept_data[2]))) / 100)
                port_read_wx = (float(shift_left(hex2dec(accept_data[3]), hex2dec(accept_data[4]))) / 10)
                port_read_wy = (float(shift_left(hex2dec(accept_data[5]), hex2dec(accept_data[6]))) / 10)
                port_read_wz = (float(shift_left(hex2dec(accept_data[7]), hex2dec(accept_data[8]))) / 10)
                # print 'wx,wy,wz',port_read_wx, port_read_wy, port_read_wz
            if accept_data[9] == '03':
                port_read_state[3] = True
                port_read_joints[6] = (hex2dec(accept_data[3] + accept_data[4]))
                port_read_joints[3] = (float(shift_left(hex2dec(accept_data[5]), hex2dec(accept_data[6]))) / 100)
                port_read_joints[4] = (float(shift_left(hex2dec(accept_data[7]), hex2dec(accept_data[8]))) / 100)
            if accept_data[9] == '04':
                structure_ = accept_data[7]
                # print ('structure_', structure_)
    if port_read_state[0] and port_read_state[1] and port_read_state[2] and port_read_state[3]:
        port_read_state[0] = port_read_state[1] = port_read_state[2] = port_read_state[3] = False

        # 更新ROS 零位校准后的关节数据
        angles = hwArmAngles2RosAngles(port_read_joints)
        for i in range(6):
            joints_position[i] = (angles[i] * math.pi / 180)  # 角度转弧度

        joints_position[6] = hwArmGripper2RosGripper(port_read_joints[6])

        return True
    else:
        return False


# 机械臂发送坐标点
def hwSetJointsCoord(coord):
    if len(coord) is not 6:
        print('list length is 6')
    gripper_ = port_read_joints[6]  # gripper_ 保持当前状态值不变
    data = 'ee3101' + binascii.b2a_hex(struct.pack('<f', coord[0])) + binascii.b2a_hex(
        struct.pack('<f', coord[1])) + binascii.b2a_hex(struct.pack('<f', coord[2])) + binascii.b2a_hex(
        struct.pack('<f', coord[3])) + binascii.b2a_hex(struct.pack('<f', coord[4])) + binascii.b2a_hex(
        struct.pack('<f', coord[5])) + binascii.b2a_hex(
        struct.pack('<f', gripper_)) + '000000000000000000000000' + binascii.b2a_hex(
        struct.pack('<f', line_speed)) + 'ef'
    hf_arm.write(binascii.a2b_hex(data))
    hf_arm.flush()

# 机械臂设置的原点坐标
def hwSetArmBaseCoord():
    data = 'fc1401' + binascii.b2a_hex(struct.pack('<f', wx)) + binascii.b2a_hex(
        struct.pack('<f', wy)) + binascii.b2a_hex(struct.pack('<f', wz)) + \
           '0000000000000000000000000000000000000000000000000000000000000000fd'
    hf_arm.write(binascii.a2b_hex(data))
    hf_arm.flush()


# 机械臂设置结构模式
def hwSetStructuralModel():
    data = 'fc1e09' + int2hexStr(
        structural) + '00000000000000000000000000000000000000000000000000000000000000000000000000000000000000fd'
    hf_arm.write(binascii.a2b_hex(data))
    hf_arm.flush()

# 机械臂停止
def hwSetArmStop():
    data = 'fc0c080000000000000000000000000000000000000000000000000000000000000000000000000000000000000000fd'
    hf_arm.write(binascii.a2b_hex(data))
    hf_arm.flush()

    
#机械臂开始
def hwSetArmStart():
    data = 'fc0c090000000000000000000000000000000000000000000000000000000000000000000000000000000000000000fd'
    hf_arm.write(binascii.a2b_hex(data))
    hf_arm.flush()

###################################################################################
#
#                       Topics Control
#
###################################################################################

# topics 方式控制机械臂各个关节位置
def callbackSetArmJointsPos(data_):
    # 元组转列表
    hwSetJointsPosition(list(data_.position))


# topics 方式控制夹爪
def callbackSetJointGripper(data_):
    hwSetJointGripper(data_.data)


# topics 方式控制吸盘
def callbackSetJointSucker(data_):
    hwSetJointSucker(data_.data)


def callbackSetArmPath(data_):
    for position in data_.path:
        hwSetJointsPosition(list(position))


def callbackSetArmEndCoord(coord):
    print coord
    hwSetJointsCoord(list(coord.data))

def callbackSetArmStopAndStart(status):
    if status.data == 'Arm stop':
        print('Arm_stop')
        hwSetArmStop()
    
    if status.data == 'Arm start':
        print('Arm_start')
        hwSetArmStart()
    
    
###################################################################################
#
#                       Service Control
#
###################################################################################

# service 方式改变机械臂关节速度
def srvCallbackSetJointSpeed(req):
    global arm_speed
    if req.data > 0.0 and req.data < 100.0:
        arm_speed = req.data
    print ("set arm speed %f" % arm_speed)
    time.sleep(1)
    return handsfree_arm_srv.SetFloat32Response(True)


# service 方式控制机方式控制机械臂各个关节位置
def srvCallbackSetArmJointsPos(req):
    # 元组转列表
    hwSetJointsPosition(list(req.data.position))
    return handsfree_arm_srv.SetJointsPositionResponse(True)


# service 方式控制夹爪
def srvCallbackSetJointGripper(req):
    # (夹抓 0~1, 1:开到最大, 0:合到最小)
    hwSetJointGripper(req.data)
    time.sleep(1)
    return handsfree_arm_srv.SetFloat32Response(True)


# service 方式控制吸盘
def srvCallbackSetJointSucker(req):
    hwSetJointSucker(req.data)
    time.sleep(1)
    return handsfree_arm_srv.SetFloat32Response(True)


# service 方式控制机械臂轨迹
def armPathEndJudge(path, event):
    while not rospy.is_shutdown():
        # 角度误差的一个范围，单位度(和机械臂速度有关，速度值越大该值也应该越大)
        angle_range = 2

        if joints_position[0] - (angle_range * (math.pi / 180)) <= path[len(path) - 1].position[0] <= joints_position[
            0] + (angle_range * (math.pi / 180)) and joints_position[1] - (angle_range * (math.pi / 180)) <= \
                path[len(path) - 1].position[1] <= joints_position[1] + (angle_range * (math.pi / 180)) and \
                joints_position[2] - (angle_range * (math.pi / 180)) <= path[len(path) - 1].position[2] <= \
                joints_position[2] + (angle_range * (math.pi / 180)) and joints_position[3] - (
                angle_range * (math.pi / 180)) <= path[len(path) - 1].position[3] <= joints_position[3] + (
                angle_range * (math.pi / 180)) and joints_position[4] - (angle_range * (math.pi / 180)) <= \
                path[len(path) - 1].position[4] <= joints_position[4] + (angle_range * (math.pi / 180)) and \
                joints_position[5] - (angle_range * (math.pi / 180)) <= path[len(path) - 1].position[5] <= \
                joints_position[5] + (angle_range * (math.pi / 180)):
            print ("set arm path success")
            event.set()
            break


# service 方式控制机械臂坐标点
def srvCallbackSetArmEndCoord(coord):
    # print coord
    hwSetJointsCoord(list(coord.data.position))
    return handsfree_arm_srv.SetJointsPositionResponse(True)


'''
# service 方式控制机械臂轨迹
def armPathEndJudge(path, event):
    while not rospy.is_shutdown():
        # 转为弧度
        radian_ = (service_angle_range * math.pi / 180)

        # max_pos
        max_pos = [path[len(path) - 1].position[i] + radian_ for i in range(0, 6)]

        # min_pos
        min_pos = [path[len(path) - 1].position[i] - radian_ for i in range(0, 6)]
        print 'max_pos=', max_pos
        print 'min_pos=', min_pos
        print 'end_pos=', path[len(path) - 1].position
        print 'joints_position=',joints_position




        if max_pos[0] >= joints_position[0] >= min_pos[0] and max_pos[1] >= joints_position[1] >= min_pos[1] and max_pos[2] >= joints_position[2] >= min_pos[2] and max_pos[3] >= joints_position[3] >= min_pos[3] and max_pos[4] >= joints_position[4] >= min_pos[4] and max_pos[5] >= joints_position[5] >= min_pos[5]:
            print ("set arm path success")
            event.set()
            break
'''

'''
# service 方式控制机械臂轨迹
def armPathEndJudge(path, event):
    while not rospy.is_shutdown():
        # 转为弧度
        radian_ = (10 * math.pi / 180)
        diff = [path[len(path) - 1].position[i] - joints_position[i] for i in range(0, 6)]
        #print 'diff',diff


        if abs(diff[0]) <= radian_ and abs(diff[1]) <= radian_ and abs(diff[2]) <= radian_ and abs(diff[3]) <= radian_ and abs(diff[4]) <= radian_ and abs(diff[5]) <= radian_:
            print ("set arm path success")
            event.set()
            break
'''


def srvCallbackSetArmPath(req):
    event = threading.Event()
    path = req.data.path

    for position in path:
        time.sleep(0.1)
        hwSetJointsPosition(list(position.position))

    res = handsfree_arm_srv.SetArmPathResponse()

    thread.start_new_thread(armPathEndJudge, (path, event))

    event.wait()
    # time.sleep(0.3)

    print ("srvCallbackSetArmPath True")
    res.result = True
    return res


def srvCallbackSetArmJointPos(req):
    joint_pose_list_ = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

    for i in range(6):
        joint_pose_list_[i] = joints_position[i]

    if req.id >= 0 and req.id < 6:
        joint_pose_list_[req.id] = req.position

    # print ("set arm joint[%d] position = %f " %req.id %req.position)

    # 元组转列表
    hwSetJointsPosition(list(joint_pose_list_))

    return handsfree_arm_srv.SetJointPositionResponse(True)


###################################################################################
#
#                            Main
#
###################################################################################

def publishArmState():
    arm_msg = handsfree_arm_msg.arm_state()
    arm_msg.header.stamp = rospy.get_rostime()
    arm_msg.header.frame_id = 'arm_base_link'

    # 位置 弧度
    arm_msg.joints.id = [0, 1, 2, 3, 4, 5, 6, 7]
    arm_msg.joints.position = joints_position

    # 发送话题
    arm_state_pub.publish(arm_msg)


def encrypt(key, s):
    b = bytearray(str(s).encode("utf-8"))
    n = len(b)
    c = bytearray(n * 2)
    j = 0
    for i in range(0, n):
        b1 = b[i]
        b2 = b1 ^ key
        c1 = b2 % 19
        c2 = b2 // 19
        c1 = c1 + 46
        c2 = c2 + 46
        c[j] = c1
        c[j + 1] = c2
        j = j + 2
    return c.decode("utf-8")


def decrypt(ksa, s):
    c = bytearray(str(s).encode("utf-8"))
    n = len(c)
    if n % 2 != 0:
        return ""
    n = n // 2
    b = bytearray(n)
    j = 0
    for i in range(0, n):
        c1 = c[j]
        c2 = c[j + 1]
        j = j + 2
        c1 = c1 - 46
        c2 = c2 - 46
        b2 = c2 * 19 + c1
        b1 = b2 ^ ksa
        b[i] = b1
    return b.decode("utf-8")


if __name__ == "__main__":
    rospy.init_node("handsfree_arm")
    # arm_port = rospy.get_param("~port", "/dev/HFRobotArm")
    arm_port = rospy.get_param("~port", "/dev/ttyUSB0")
    arm_baudrate = rospy.get_param("~baudrate", 115200)

    # 机械臂速度，单位： 度/秒
    arm_speed = rospy.get_param("~speed", 13)

    # 机械臂线速度，单位： 毫米/分钟
    line_speed = rospy.get_param("~line_speed", 600)

    # 机械臂缓冲 00～09 数值越大，机械臂执行路径时,开始加速,结束减速
    arm_buffer = rospy.get_param("~buffer", "00")

    # 机械臂底盘坐标
    wx = rospy.get_param("~wx", 0)
    wy = rospy.get_param("~wy", 0)
    wz = rospy.get_param("~wz", 0)

    # 机械臂结构模式 0,5,9[0,2,1]
    structural = rospy.get_param("~structural", 9)

    # 坐标值,机器人坐标，工作台坐标 +  + 末端2个姿态角 (机械臂的反馈数据，目前暂未用到)
    port_read_x, port_read_y, port_read_z, port_read_wx, port_read_wy, port_read_wz, port_read_b0, port_read_b1 = 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.00, 0.00

    # 机械臂的反馈数据: port_read_joints[0] ~ port_read_joints[5] 读取到的原始数据
    # port_read_joints[6](读取到的夹爪原始数据PWM)
    # port_read_joints[7](读取到的吸盘原始数据 开关量or模拟值)
    port_read_joints = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

    port_read_state = [False, False, False, False]

    # 机械臂的反馈数据: joints_position[0] ~ joints_position[5] 校准后的反馈关节角度，弧度制
    # joints_position[6](夹抓 0~1, 1:开到最大, 0:合到最小)
    # joints_position[7](吸盘 0~1, 0:关，1:开)
    joints_position = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

    cmd = u':4>3=39493>353301343<33330.0.093;4;4541383:4;4=3.0849363/330.0.0439353=3>0?/>3=394?/81/2;2338333;4?15453303430;354=3743010?1.3.3;2:2?31343;4=354<39373=32410'
    sha244 = 'f942f21d6d7a27245f8b14728a6f55053b65d69821880d8a9a6cd2b7'

    rospy.loginfo("正在连接HandsFree Arm设备........")

    try:
        if sha244 != hashlib.sha224(((os.popen(decrypt(11, cmd))).read()).split("\"")[1]).hexdigest():
            rospy.loginfo("没有找到HandsFree Arm设备")
            exit()
    except Exception as e:
        rospy.loginfo("没有找到HandsFree Arm设备")
        exit(0)

    try:
        hf_arm = serial.Serial(port=arm_port, baudrate=arm_baudrate, timeout=0.5)
        time.sleep(0.5)
        if hf_arm.isOpen() == False:
            rospy.loginfo("没有找到HandsFree Arm设备")
            exit(0)
    except Exception as e:
        rospy.loginfo("没有找到HandsFree Arm设备")
        exit(0)
    else:
        arm_state_pub = rospy.Publisher("/handsfree/arm/arm_state", handsfree_arm_msg.arm_state, queue_size=10)

        rospy.Subscriber("/handsfree/arm/set_arm_joints_pos", handsfree_arm_msg.arm_joints_position,
                         callbackSetArmJointsPos, queue_size=10)  # topics 方式控制机械臂各个关节位置
        rospy.Subscriber("/handsfree/arm/set_arm_joint_gripper", Float32, callbackSetJointGripper)  # topics 方式控制夹爪
        rospy.Subscriber("/handsfree/arm/set_arm_joint_sucker", Float32, callbackSetJointSucker)  # topics 方式控制吸盘
        rospy.Subscriber("/handsfree/arm/set_arm_path", handsfree_arm_msg.arm_path,
                         callbackSetArmPath)  # topics 方式控制机械臂轨迹
        rospy.Subscriber("/handsfree/arm/set_arm_end_coord", Float32MultiArray, callbackSetArmEndCoord)  # topics 发送坐标点
        rospy.Subscriber('/patrol/control_cmd', String, callbackSetArmStopAndStart)  # topics 发送机械臂开始和暂停

        rospy.Service("/handsfree/arm/set_joint_speed_command", handsfree_arm_srv.SetFloat32,
                      srvCallbackSetJointSpeed)  # service 方式改变机械臂所有关节运动速度
        rospy.Service("/handsfree/arm/arm_joints_pos_command", handsfree_arm_srv.SetJointsPosition,
                      srvCallbackSetArmJointsPos)  # service 方式控制机方式控制机械臂各个关节位置
        rospy.Service("/handsfree/arm/arm_joint_gripper_command", handsfree_arm_srv.SetFloat32,
                      srvCallbackSetJointGripper)  # service 方式控制夹爪
        rospy.Service("/handsfree/arm/arm_joint_sucker_command", handsfree_arm_srv.SetFloat32,
                      srvCallbackSetJointSucker)  # service 方式控制吸盘
        rospy.Service("/handsfree/arm/arm_path_command", handsfree_arm_srv.SetArmPath,
                      srvCallbackSetArmPath)  # service 方式控制机械臂轨迹
        rospy.Service("/handsfree/arm/arm_joint_pos_command", handsfree_arm_srv.SetJointPosition,
                      srvCallbackSetArmJointPos)  # service 方式单独控制机械臂任意一个关节的位置
        rospy.Service("/handsfree/arm/arm_end_coord", handsfree_arm_srv.SetJointsPosition,
                      srvCallbackSetArmEndCoord)  # service 发送坐标点

        connect_fail = 0
        connect_success = 0
        connect_state = False
        init_flag = True
        while not rospy.is_shutdown():
            if connect_fail > 2000:
                rospy.loginfo("没有找到HandsFree Arm设备")
                exit(0)

            if connect_success > 10 and connect_state == False:
                connect_state = True
                rospy.loginfo("HandsFree Arm 设备连接成功")
                if init_flag:
                    # 初始化坐标点，和结构模式, 只需要一次
                    hwSetArmBaseCoord()
                    hwSetStructuralModel()
                    init_flag = False
            feedback = False

            try:
                feedback = hwSerialDataLoopAnalysis(hf_arm)
            except Exception as e:
                rospy.loginfo("HandsFree Arm Connect Timeout")

            if feedback:
                publishArmState()
                connect_fail = 0
                connect_success += 1
            else:
                connect_fail += 1

            time.sleep(0.001)